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Abstract 

In addition to the theoretical value of challenging optimal 
control problmes, recent progress in autonomous vehicles 
mandates further research in optimal motion planning for 
wheeled vehicles. Since current numerical optimal control 
techniques suffer from either the curse of dimensional- 
ity, e.g. the Hamilton- Jacobi-Bellman equation, or the 
curse of complexity, e.g. pseudospectral optimal con- 
trol and max-plus methods, analytical characterization 
of geodesies for wheeled vehicles becomes important not 
only from a theoretical point of view but also from a prac- 
tical one. Such an analytical characterization provides a 
fast motion planning algorithm that can be used in robust 
feedback loops. In this work, we use the Pontryagin Max- 
imum Principle to characterize extremal trajectories, i.e. 
candidate geodesies, for a car-like robot with one trailer. 
We use time as the distance function. In spite of partial 
progress, this problem has remained open in the past two 
decades. Besides straight motion and turn with maxi- 
mum allowed curvature, we identify planar elastica as the 
third piece of motion that occurs along our extremals. We 
give a detailed characterization of such curves, a special 
case of which, called merging curve, connects maximum 
curvature turns to straight line segments. The structure 
of extremals in our case is revealed through analytical 
integration of the system and adjoint equations. 

1 Introduction 

With the advent of autonomous vehicles, there is 
a renewed need for analytical characterization of 
geodesies for wheeled vehicles, as this is an impor- 
tant problem not only from a theoretical point of 
view but also from a practical one. Such charac- 
terizations provide motion planning algorithms with 
constant time complexity that can be used in a ro- 
bust feedback loop. Moreover, geodesies for wheeled 
vehicles provide a library of motion primitives from 
which collision-free (locally) optimal paths can be 
constructed in the presence of obstacles. Existing 
alternative methods, namely numerical optimal con- 
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Figure 1: The configuration (x,y,9,/3) and controls 
(y, uS) of a car-like robot with one trailer. 



trol techniques, suffer from either the curse of dimen- 
sionality, e.g. the Hamilton- Jacobi-Bellman equation 
requires 0(n d ) space for a <i-dimcnsional system [3], 
or the curse of complexity, e.g. pseudospectral opti- 
mal control algorithms that solve a general finite di- 
mensional nonlinear optimization per initial-goal pair 
[TTJ Q3J EH1 [21] and max-plus methods that approx- 
imate the solution of the Hamilton- Jacobi-Bellman 
equation, without an explicit representation of the 
discretized configuration space [HI [T7] . 

In particular, characterization of time optimal 
paths for a car-like robot with trailers as the pre- 
mier type of geodesies has remained an open problem 
in spite of partial progress in the past two decades. 
Chyba and Sekhavat gave the first approach toward 
characterization of time optimal paths for a mobile 
robot with one trailer [9]. They applied the Pon- 
tryagin Maximum Principle and switching structure 
equations as a necessary condition for optimality to 
give a partial characterization of extremals, i.e. those 
trajectories that satisfy the necessary condition and 
hence are candidate geodesies [18l[24]. In this paper, 
we continue that work to give a complete character- 
ization of extremals with analytical solution for the 



system and adjoint differential equations. In partic- 
ular, we demonstrate that a special maneuver, which 
we call merging, comes in between a minimum ra- 
dius turn and straight line motion along time opti- 
mal paths to straighten the trailer completely before 
merging into the straight line segment. It has not 
escaped our attention that the merging curve here 
happens to be the same as planar elastica [5] [THl [35] . 
This interesting coincidence requires further investi- 
gation. 

1.1 Related Work. In a classical paper in 1957, 
L.E. Dubins characterized the shortest paths, mea- 
sured in the Euclidean plane, between two points 
in an obstacle- free SE(2), with the constraint that 
the average curvature of the projection of the path 
onto the Euclidean plane over any interval along the 
path be bounded from above [10] . The Dubins curves 
are composed of sequences of up to three primitives, 
which consists in a line and an arc of a circle with 
maximum allowed curvature. They are of particu- 
lar interest in control and robotics because they give 
the shortest paths for a simple airplane model as well 
as a forward-only car with bounded steering angle 
and velocity. Extending the car model to allow back- 
ward motion as well, Reeds and Shepp characterized 
the shortest paths for a car with bounded steering 
angle and velocity [19]. Sussmann and Tang devel- 
oped a general methodology and machinery based 
on geometric optimal control for solving problems 
of this sort by revisiting the Reeds-Shepp problem 
[2"3j . Soueres, Boissonnat, and Laumond gave an op- 
timal control synthesis, i.e. a mapping from pairs of 
initial-goal configurations to the optimal trajectories 
[22] [23] . These developments lead in the new mille- 
nium to the characterization of time optimal trajecto- 
ries for the differential drive [2] and omni-directional 
vehicles in the plane [T] [T^J [T3] . Chyba et al. con- 
sidered time optimal trajectories for an underwater 
vehicle [7] \8\. We characterized time optimal trajec- 
tories for a simple 3D airplane model obtained by 
extending the Dubins car model [5J and geodesies for 
a differential drive robot with the total wheel rotation 
as the distance function HI 



2 Problem Specification 

In this paper, we consider the analytical solution of 
the following optimal control problem for every initial 
and goal configuration pair qi,q g : 
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subject to q(0) = qi, q(T) = q g , and 



(2.2) 



in which q = (x,y,0,fi) G R 2 x S 1 x S 1 is the 
configuration and u = (v,uj) G U = [— 1,1] 2 is the 
control. Figure Q] depicts the configuration of the 
robot, where v is the linear velocity, to is the angular 
velocity, (x, y) is the coordinate of the center of robot, 
9 is the orientation, and j3 is the angle between the 
trailer and robot. 

3 Pontryagin Maximum Principle 

Let A = (X x , A y , Xg, \p) be the adjoint, and 

(3.3) 

H(q, A, u) = X x ± + X y y + X g 9 + A^/3 
= v(X x cos 6* + X y sin 6* — Xp sin/3) + uj(Xg + Xp), 

the Hamiltonian. The Pontryagin Maximum Prin- 
ciple [18] ensures that for every optimal trajectory- 
control pair (q(t),u(t)) over the time interval [0,T], 
there exists an absolutely continuous vector-valued 
adjoint function X(t) ^ such that 
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and H is maximized as a function of v,ui along the 
optimal trajectory, i.e. for all t G [0,T], 

(3.8) H(q(t), X(t),u(t)) = max H(q(t),X(t), z). 
Direct integration with help from (|2.2[) yields 



(3.9) 

(3.10) 

(3.11) 

(3.12) 



A x = ci, 

Xy = C 2 , 

Ae = c x y - c 2 x + c 3 , 
Xp = C4 cxp I / ucos/3(r) dr ) , 



(2.1) 



dt 



in which ci, 02,03,04 = Xp(§) are constant. Further 
integration of Xp requires more analysis which will be 
presented in the next sections. 



Since the Pontryagin Maximum Principle guar- 
antees that the Hamiltonian is maximized, as a func- 
tion of the controls, we can define switching functions 

(3.13) (j) v = X x cos 6* + A y sin# — \p sin/3, 

(3.14) w = A e + A /3 , 

such that V(f> v = \<f> v \ and w</> w = That is 

v = sgn(0„) whenever <j> v =^ and lu = sgn(0 w ) 
whenever <j> u ^ 0. 

An optimal trajectory is bound to satisfy the 
Pontryagin Maximum Principle, but not every trajec- 
tory that satifies the Pontryagin Maximum Principle 
is necessarily optimal. A trajectory that satisfies the 
Pontryagin Maximum Principle is called an extremal. 
Extremal trajectories can be divided into two cate- 
gories: regular and singular. 

4 Regular Extremals 

Definition 4.1. An extremal trajectory is called 
regular if the times at which <f) v — or tp u = have 
zero measure, i.e. <j) v ^ and <puj almost every- 
where. We define a regular primitive to be a subtra- 
jectory of a regular extremal, along which <f> v ^ and 
^ ± 0. ' 

Depending on the signs of the switching func- 
tions, there are four types of regular primitives: 

• Forward-Left: t) = l,w=l, 



Forward- Right: 



-1, 



Backward-Left: v = — 1,lu = 1, 



Backward- Right: v = —1,lu = — 1. 



Let [io,^i] C [0,T] be the time interval over which a 
regular primitive is defined. Since the controls v and 
lu are constant and fJ — lu — v sin /?, we can replace 
dr with d(3 / {lu — v sin (3) in (|3~T2|) to obtain 



\p(t) = \p(t )exp 



rP(t) 



v cos ft 



(4.15) 



Xp(to 



lu — v sin/3(io) 
u — v sinft(t) 



v sin ft 



d/3 



Note that ft will never reach (v/lu)it/2 along a regular 
primitive if it does not start at (v/lu)tt/2, in which 
case our change of variables is valid. If ft (to) = 
(v/w)ir/2, then /3(t ) = (v/lu)it/2 and \p(t) = 



\p(to). Therefore along a regular primitive, 



tut + 9(t ), 

x(to) + (v/u)(eaa(9) -sin6(t )), 
y{t ) - (v/w)(cos(0)-coB0(to)), 
't-2v + Ki 



(4.16) 


9{t) 


(4.17) 


x(t) 


(4.18) 


y(t) 


(4.19) 


(3(t) 


(4.20) 





2(v/lu) arctan 
lu — v sin ft{t) 



t + A'i 



where constants K\ and K 2 depend on the initial 
configuration by 



(4.21) 
(4.22) 



K 



w-wtan(/3(to)/2)' 
K 2 = \p(t )(Lu-v sin P(t )). 



5 Singular Extremals 

Definition 5.1. Extremals along which both <j> v = 
and <j> u = are called abnormal. An extremal 
trajectory is called singular if it contains a positive- 
measure subtrajectory along which <f> v = or W = 
but not both. We call such a subtrajectory a singular 
primitive . 



Chyba and Sekhavat characterized singular primi- 
tives [9]. In particular, they showed that abnormal 
extremals are either trivial or satisfy v = 0,lu = ±1, 
which coincides with a special case of <p v -singular 
primitives below. We will summarize Chyba and 
Sekhavat's results below with a slight correction for 
^-singular primitives. 

(/)„-singular primitives. <p v = on [io,ti]: in 
this case, c\ = c 2 = c± = and A = (0,0, C3,0). 
Controls arc lu = ±1 and v G [—1,1] arbitrarily. 

</> w -singular primitives. (f> u = on [to,ii]: in 
this case, <p v = cte ^= and A^ = — Xg. Since 0^ = 0, 



(5.23) 



Ae + \p 

v(a sin 6* — c 2 cos9 + Xp cos f3) = 0. 



Since v = sgn(0„) ^ 0, c\ sin 9 — c 2 cos 9 + Xp cos ft = 
0. Taking derivative with respect to time, we obtain 
(5.24) 



= (ci cos 9 + c 2 sin 9)6 
= lu(ci cos 9 + c 2 sin 9 

= LU(j) v + llXp 
= w\(j) v \ + X/3, 



+ X/3 cos f3 — Xpft sin ft 
— X/3 sin/3) + vXfj 



using the fact that v = sgn(<j> v ) and (|2.2|) , (|3.7[) , (|3.9[) , 
(|3TT0l) . (j3~T3l) . and (j3~T4l) . Hence, 

A* 




(5.25) 



Cij/ - c 2 x + c 3 
10,1 ' 



using the fact that A/j = —Xg and (|3.11|) . Note that 
Chyba and Sekhavat have made a minus sign mis- 
take in these calculations [9]. Next, we analytically 
integrate the system over a </> w -singular primitive. 
It is obvious from (12.21) that 



.r = UJ 



dx 
dy 



(5.26) 

(5.27) y = uj^-=vsm9. 

do 

Hence using ()2.2|) and (|5.25j) . we obtain 

(5.28) (ciy — c 2 x 

(5.29) (c lV - c 2 x 
Integrating with respect to 9, we get 



. dx 
C3) d6 = 
^dy 



cos( 



sm ( 



(5.30) 2 
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C2X L 



c 3 x\ to +C1 
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(5.31) 2 



ie(t ) dv 
4> v {s'md(t) — sin9(to)} , 

x Te de 



ciy 2 \\ +c 3 y|' - pa 



'e(to) 

^ {cos 6> (to) — cos6*(i)} . 



Multiply (pT3T)| by c 2 and add to (jOTj) multiplied by 
— Ci to obtain 
(5.32) 
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Figure 2: A singular extremal trajectory composed of 
a (j> u -singular primitive, plotted in red, and regular 
primitives, plotted in blue. In this case, the 4> u - 
singular primitive does not contain a straight line 
segment. The line I is defined by c\y — c 2 x + c 3 = 0. 

Therefore, the following lemma holds. 

Lemma 5.1. Consider a ip^- singular primitive over 



the time interval [to,ti] 
(5.33) 

Xg{t) = 4> v sin/3(t) 



In that case for t G [to, ti], 



± 



^A 2 (t ) - 2</> v Xg {t ) sin f3 (t ) + </> 2 sin 2 /3 (t) . 

Figure [2] illustrates a singular extremal contain- 
ing a </> w -singular primitive. Particularly, we are in- 
terested in those extremals that contain straight line 
motions, i.e. v — ±l,u; = 0, since they are the only 
ones that can optimally take the robot to far desti- 
nations. A straight motion occurs only when Xg = 
and 9 = arctan(c2/ci) or 9 = ir + arctan(c2/ci). For 
the trajectory to cross the line Xe = at some time 
in [to,ti], (|5.32[) necessitates that either Xg(to) = or 

5-34 <Pv = . M . v 

2 sm p(to) 

This is necessary but not sufficient as the control 
has to be within the valid range, i.e. \uj\ < 1, for 
the trajectory to remain singular and reach the line 
Xg = 0. In particular, if Xg(to) ^ 0, then sin (3(t ) ^ 
and for \u>\ < 1 we must have | sin /3(t ) < \ which is 
equivalent to 

(5.35) - - < (3(t ) < - or — < (3{t ) < — . 



1/ 

2 {ClV 



C2X) 2 \\ 



c?,{ciy - c 2 x)\\ 



cl)\\ 



= l{$(to)-\ 2 g(t)} 

= 4> v {ci cos 9(t) + c 2 s'm9(t) — c\ cos 9(t ) 
= (f> v {Xg {t ) sin/? {t ) - Xg(t) Sill /?(£)} . 



Figure 4: A merging curve followed by a straight 
motion and another merging curve. This case is a 
^-singular primitive that contains a segment of the 
line i : c\y — c 2 x + c 3 = 0. 

For the moment assume Xg(t ) ^ 0. Note that 

• au ftom (13.131) we have 
c 2 sm9(t ]} " v 

(5.36) 4> v — a cos 9 + c 2 sin 9 + Xg sin/3, 



Figure 3: A singular extremal trajectory composed of a (^-singular primitive, plotted in red, and regular 
primitives, plotted in blue. In this case, the </> w - singular primitive contains a straight line segment. The line 
I is defined by C\y — c 2 x + c 3 = 0. 



which yields which implies 
(5.37) 

4> v =c lC os6(to)+c 2 sm6(t ) + \ e (to)sml3(t ). ( 5 ' 45 ) a = ±2 ^°) or tt ± 2/3(t„). 

Together with (ES), this equation yields an equation Dcnotc the line A e = by I, and the signed distance 

for A e (i ) in terms of 0(t o ) and /3(i„): from i x ' V) to 1 ^ 



(5.38) 

Xe(to) cos2/3(t ) = 2 sin (3(t ) {a cos0(t o ) + c 2 sin 0(t o )}C 5 - 46 ) 



ciy - c 2 x + c 3 



l(ci,c 2 ) 



Therefore, 
(5.39) 

As (to) = 



2 sin^(t ){ci cos 9(t ) + c 2 sin 6»(t )} 



Figure O illustrates d. From (|3.11l) and (|5.34p , we 
obtain 



cos2/3(t ) 

as (|5.35[) guarantees that cos2/3(t ) 7^ and 
Ci cos0(t o ) + c 2 sin0(t o ) 



(5.47) 



\d(t )\ = 2|sin/?(t )|. 



Moreover, the extremal angular control in (|5.25[) 
becomes 



(5.40) 



cos 2(3(t ) 



(5.48) 



from (15341) . 

Now for the trajectory to join the line A# = 
tangentially so that it is able to follow a straight 
motion, such as in Figures |4] and El the orientation 



cyy - c 2 x + c 3 
ll(ci,c 2 )|| 



d. 



It is now obvious that for \u>\ < 1, the distance 
of the robot from I has to remain within [—1,1]. 
Since to was not assumed to be the time at which 



, , i , • , 1 u 1 a singular primitive starts, our results above hold for 

vector (cos 0, sin 6) at the tangency point should be 6 ^ ' 

±(c 1 , C2 )/||(c 1 , C2 )||. Hence, yields a11 f e ^ and hcnce the followin S lemma - 

(5.41) Lemma 5.2. Let I denote the line ay — c 2 x + C3 = 

<M±ll(ci,c 2 )|| -d cos 0(to) -c 2 sin0(t o )} = -A^(t ), * n </le ^ flne ' arl,i coWer a ^singular primitive 

2 over t/ie time interval [to,ti] t/iat contains a straight 

from which we can derive two equations for ci, c 2 , and Zw*e segment. First, all of the straight line segments 
C3 in terms of x(to), y(to), 0(to)j and (3(to): 



(5.42) 

<j> v {±||(ci,c 2 
1 



of the considered primitive lie on I. Second, either 
C4 = and the entire primitive is a straight line, i. e. 



1 



d cos (t ) - c 2 sin0(t o )} = -\j(to) 



(5.49) 



^ {ay(to) - c 2 x(t ) + c 3 } , 
and from ([OB) and ([B~i0]) . 

(5.43) ^ = ±l|( Cl ,c 2 )|| 



or 



(ci,c 2 ) cosa 



(5.50) 
(5.51) 



a = or 7r, 
w = 0, 
A/3 = A e = 0, 

a(i) = ±2/3(t), 
w(i) =d = ±2sin/3(t), 



7T „, . 7T 57T . 7n 

'g </3(t) <- or — </3(t) < — , 



cos 2/3(t ) 

in which a is the angle between the orientation vector (5.52) 
at to, i.e. (cos0(to),sin0(to)), and (ci,c 2 ). Figure[5] 
illustrates a. It is clear then that m wMch d ls the sl 9 ned distance from the center 

of robot to t, and a is the angle between the robot 
(5.44) cos2/3(to) = ±cosa, orientation and i. Figure^ illustrates the latter. 



Figure 5: The robot configuration along a merging curve. In this case, the robot tangcntially joins the line 
I : ciy - c 2 x + c 3 = 0, a = ±2/3, d = ±2sin/3, and -f</3<for^<^<^ along the trajectory. 



The former case above corresponds to (X x , X y , Xe, 0) 
adjoints which pertain to the Reeds-Shepp extremals 
[2~I] including the Reeds-Shepp curves [19] . It is clear 
that the Reeds-Shepp curves are time optimal for our 
system. The latter case corrsponds to planar elastica 
connecting turning to straight segments [16] . 

Definition 5.2. We call the latter case in Lemma 
\5.2\ a merging curve. 

6 Conclusion and Future Work 

Although a complete characterization of optimal tra- 
jectories and control synthesis remains for future, we 
paved the way for such a complete characterization in 
this work. Based on this work, it is easy now to prove 
that for far enough destinations the optimal trajec- 
tory has to contain a straight line motion. Partic- 
ularly, regular primitives and </>„-singular extremals 
cannot optimally take the robot to far destinations 
as they impose constant rotation (lu = ±1) which be- 
comes non-optimal after a certain period. We char- 
acterized the ^-singular extremals which are the 
only extremals that can possibly contain a straight 
line segment. We gave a detailed characterization 
of merging curves that connect regular extremals to 
straight line segments. It is obvious that the Reeds- 
Shepp curves are optimal for our system if the initial 
and goal j3 are unimportant. The only optimal trajec- 
tories that contain a straight line segment and do not 
use merging curves are the Reeds-Shepp extremals. 
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